
/*
  Dummy node for testing base_ctrl node
*/

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

geometry_msgs::Twist msg;

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "dummy_command");
  ros::NodeHandle n;
  
  ros::Publisher pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
  int count = 150;
  msg.linear.x = 0;
  msg.angular.z = 0;
  ros::Rate loop_rate(20);
  while(1)
    {
      pub.publish(msg);
      msg.linear.x += .05;
      if (count)
	count--;
      else
	break;

      ros::spinOnce();
      loop_rate.sleep();
    }
  
  count = 170;
  while(1)
    {
      pub.publish(msg);
      msg.linear.x -= .05;
      if (count)
	count--;
      else
	break;

      ros::spinOnce();
      loop_rate.sleep();
    }

  msg.linear.x = 0;
  count = 100;
  while(1)
    {
      pub.publish(msg);
      msg.angular.z += .1;
      if (count)
	count--;
      else
	break;
      ros::spinOnce();
      loop_rate.sleep();
    }
  count = 100;
  while(1)
    {
      pub.publish(msg);
      msg.angular.z -= .1;
      if (count)
	count--;
      else
	break;
      ros::spinOnce();
      loop_rate.sleep();
    }
}
